/** 
 * potentialfilds
 * Copyright (C) 2009 - Andre Luiz Billia de Miranda.
 * Copyright (C) 2009 - Luís Paulo Faina Garcia 
 * Copyright (C) 2009 - Sérgio Henrique Martini Marinello.
 *
 * potentialfilds - A basic potential field using OpenGL
 * developed in Graphcs Computer discipline.
 *
 * Authors: Andre Luiz Billia de Miranda.
 *          Luís Paulo Faina Garcia 
 *          Sérgio Henrique Martini Marinello.
 *
 * Date: Jun, 2009
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 3
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * This code was developed by students of the Computing Engineering
 * undergraduate course of Universidade de São Paulo, São Carlos/SP,
 * Brasil. The initial developers of the original code are Luís Paulo
 * Faina Garcia <lpfgarcia@gmail.com>
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
**/


// bibliotecas basicas
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

// bibliotecas para uso de sound
#include <SDL/SDL.h>
#include <SDL/SDL_mixer.h>

// biblioteca grafica 
#include <GL/glut.h>

// definição do numero de obstaculos
#define OBS 15

// definição do som 
Mix_Chunk *som = NULL;
int temp = 0; 

// anglo de visao e tipo de aspecto
GLfloat angle, fAspect;

// tamanho do plano
GLfloat xx1 = 200.0f;
GLfloat yy1 = 200.0f;
GLfloat zz1 = 200.0f;
GLsizei size = 200;

// tamanho do robo
GLfloat rx1 = 10.0f;                                                    
GLfloat rz1 = 10.0f;
GLsizei rsize = 5;

// posicao inicial da camera
GLfloat cx1 = -100.0f;
GLfloat cy1 = 300.0f;
GLfloat cz1 = -100.0f;

// tamanho do incremento nas direções x e y 
GLfloat xstep = 1.0f;
GLfloat zstep = 1.0f;

// largura e altura da janela
GLfloat windowWidth;
GLfloat windowHeight;

// matriz de obstaculos
GLfloat obs[OBS][2];

// força de repulsao sobre o robo
int fxy = 20;


// funcao de inicialização do audio - SDL
void init() {

	SDL_Init( SDL_INIT_AUDIO | SDL_INIT_TIMER);

	// initializa SDL_mixer
	Mix_OpenAudio( 22050, MIX_DEFAULT_FORMAT, 2, 1024 );
	atexit(Mix_CloseAudio);
}


// funcao de load do audio
void carregar() {

	som = Mix_LoadWAV("aaa.ogg");
}

// funcao de close dos arquivos abertos
void descarregar(){

	Mix_FreeChunk(som);

    // sai do SDL_mixer
	Mix_CloseAudio();
 
   	// sai do SDL
    SDL_Quit();
}

// funcao que toca o som
void toca(){

	Mix_PlayChannel( -1, som, 0 );
}

// desenha o plano aonde o robo irá se mover
void DesenhaPlano(){


	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glColor3f(0.5f, 0.5f, 0.5f);

	// escolhe a textura a ser usada
   glBindTexture(GL_TEXTURE_2D, getTexture(0));

	glBegin(GL_QUADS);			
		glNormal3f(0.0, 1.0, 0.0); 	
		glTexCoord2f(0.0f, 0.0f); glVertex3f(xx1-size-5, yy1, zz1-size-5);
		glTexCoord2f(1.0f, 0.0f); glVertex3f(xx1-size-5, yy1, zz1+5);
		glTexCoord2f(1.0f, 1.0f); glVertex3f(xx1+5, yy1, zz1+5);
		glTexCoord2f(0.0f, 1.0f); glVertex3f(xx1+5, yy1, zz1-size-5);
	glEnd();

}

// desenha os obstaculos no plano
void DesenhaObs(int i){

	glColor3f(1.0f, 1.0f, 1.0f);
	
	// escolhe a textura a ser usada
   	glBindTexture(GL_TEXTURE_2D, getTexture(1));  

	//piso	
	glBegin(GL_QUADS);			
		glNormal3f(0.0, -1.0, 0.0);	
		glTexCoord2f(0.0f, 0.0f); glVertex3f(obs[i][0]-rsize, 200.0, obs[i][1]-rsize);
		glTexCoord2f(1.0f, 0.0f); glVertex3f(obs[i][0]-rsize, 200.0, obs[i][1]+rsize);
		glTexCoord2f(1.0f, 1.0f); glVertex3f(obs[i][0]+rsize, 200.0, obs[i][1]+rsize);
		glTexCoord2f(0.0f, 1.0f); glVertex3f(obs[i][0]+rsize, 200.0, obs[i][1]-rsize);
	glEnd();

	// escolhe a textura a ser usada
   	glBindTexture(GL_TEXTURE_2D, getTexture(1)); 

	//parede fundo
	glBegin(GL_QUADS);			
		glNormal3f(0.0, 0.0, -1.0);	
		glTexCoord2f(0.0f, 0.0f); glVertex3f(obs[i][0]+rsize, 200.0, obs[i][1]+rsize);
		glTexCoord2f(1.0f, 0.0f); glVertex3f(obs[i][0]-rsize, 200.0, obs[i][1]+rsize);
		glTexCoord2f(1.0f, 1.0f); glVertex3f(obs[i][0]-rsize, 210.0, obs[i][1]+rsize);
		glTexCoord2f(0.0f, 1.0f); glVertex3f(obs[i][0]+rsize, 210.0, obs[i][1]+rsize);
	glEnd();

	// escolhe a textura a ser usada
   	glBindTexture(GL_TEXTURE_2D, getTexture(1));

	//parede fundo
	glBegin(GL_QUADS);			
		glNormal3f(1.0, 0.0, 0.0);	
		glTexCoord2f(0.0f, 0.0f); glVertex3f(obs[i][0]+rsize, 200.0, obs[i][1]-rsize);
		glTexCoord2f(1.0f, 0.0f); glVertex3f(obs[i][0]+rsize, 200.0, obs[i][1]+rsize);
		glTexCoord2f(1.0f, 1.0f); glVertex3f(obs[i][0]+rsize, 210.0, obs[i][1]+rsize);
		glTexCoord2f(0.0f, 1.0f); glVertex3f(obs[i][0]+rsize, 210.0, obs[i][1]-rsize);
	glEnd();

	// escolhe a textura a ser usada
   	glBindTexture(GL_TEXTURE_2D, getTexture(1));

	//parede frente
	glBegin(GL_QUADS);			
		glNormal3f(0.0, 0.0, 1.0);	
		glTexCoord2f(0.0f, 0.0f); glVertex3f(obs[i][0]+rsize, 200.0, obs[i][1]-rsize);
		glTexCoord2f(1.0f, 0.0f); glVertex3f(obs[i][0]-rsize, 200.0, obs[i][1]-rsize);
		glTexCoord2f(1.0f, 1.0f); glVertex3f(obs[i][0]-rsize, 210.0, obs[i][1]-rsize);
		glTexCoord2f(0.0f, 1.0f); glVertex3f(obs[i][0]+rsize, 210.0, obs[i][1]-rsize);
	glEnd();

	// escolhe a textura a ser usada
   	glBindTexture(GL_TEXTURE_2D, getTexture(1)); 

	//parede frente
	glBegin(GL_QUADS);			
		glNormal3f(-1.0, 0.0, 0.0);	
		glTexCoord2f(0.0f, 0.0f); glVertex3f(obs[i][0]-rsize, 200.0, obs[i][1]-rsize);
		glTexCoord2f(1.0f, 0.0f); glVertex3f(obs[i][0]-rsize, 200.0, obs[i][1]+rsize);
		glTexCoord2f(1.0f, 1.0f); glVertex3f(obs[i][0]-rsize, 210.0, obs[i][1]+rsize);
		glTexCoord2f(0.0f, 1.0f); glVertex3f(obs[i][0]-rsize, 210.0, obs[i][1]-rsize);
	glEnd();

	// escolhe a textura a ser usada
    glBindTexture(GL_TEXTURE_2D, getTexture(1)); 

	//teto
	glBegin(GL_QUADS);			
		glNormal3f(0.0, 1.0, 0.0);	
		glTexCoord2f(0.0f, 0.0f); glVertex3f(obs[i][0]-rsize, 210.0, obs[i][1]-rsize);
		glTexCoord2f(1.0f, 0.0f); glVertex3f(obs[i][0]-rsize, 210.0, obs[i][1]+rsize);
		glTexCoord2f(1.0f, 1.0f); glVertex3f(obs[i][0]+rsize, 210.0, obs[i][1]+rsize);
		glTexCoord2f(0.0f, 1.0f); glVertex3f(obs[i][0]+rsize, 210.0, obs[i][1]-rsize);
	glEnd();

}

// desenha nosso robo
void DesenhaRobo(){

	glColor3f(1.0f, 0.0f, 0.0f);
	glBegin(GL_QUADS);			
		glNormal3f(0.0, 0.0, 1.0);	
		glVertex3f(rx1+rsize, 200.0, rz1+rsize);
		glVertex3f(rx1-rsize, 200.0, rz1+rsize);
		glVertex3f(rx1-rsize, 210.0, rz1+rsize);
		glVertex3f(rx1+rsize, 210.0, rz1+rsize);
	glEnd();


	glColor3f(1.0f, 0.0f, 0.0f);
	glBegin(GL_QUADS);			
		glNormal3f(0.0, 0.0, -1.0);	
		glVertex3f(rx1+rsize, 200.0, rz1-rsize);
		glVertex3f(rx1-rsize, 200.0, rz1-rsize);
		glVertex3f(rx1-rsize, 210.0, rz1-rsize);
		glVertex3f(rx1+rsize, 210.0, rz1-rsize);
	glEnd();

	glColor3f(1.0f, 0.0f, 0.0f);
	glBegin(GL_QUADS);			
		glNormal3f(1.0, 0.0, 0.0);	
		glVertex3f(rx1+rsize, 200.0, rz1-rsize);
		glVertex3f(rx1+rsize, 200.0, rz1+rsize);
		glVertex3f(rx1+rsize, 210.0, rz1+rsize);
		glVertex3f(rx1+rsize, 210.0, rz1-rsize);
	glEnd();

	glColor3f(1.0f, 0.0f, 0.0f);
	glBegin(GL_QUADS);			
		glNormal3f(-1.0, 0.0, 0.0);	
		glVertex3f(rx1-rsize, 200.0, rz1-rsize);
		glVertex3f(rx1-rsize, 200.0, rz1+rsize);
		glVertex3f(rx1-rsize, 210.0, rz1+rsize);
		glVertex3f(rx1-rsize, 210.0, rz1-rsize);
	glEnd();


	glColor3f(1.0f, 0.0f, 0.0f);
	glBegin(GL_QUADS);			
		glNormal3f(0.0, 1.0, 0.0);	
		glVertex3f(rx1-rsize, 210.0, rz1-rsize);
		glVertex3f(rx1-rsize, 210.0, rz1+rsize);
		glVertex3f(rx1+rsize, 210.0, rz1+rsize);
		glVertex3f(rx1+rsize, 210.0, rz1-rsize);
	glEnd();


}



// função callback chamada para fazer o desenho
void Desenha(void)
{
	int i;	

	DesenhaPlano();

	for(i=0; i<OBS; i++)
		DesenhaObs(i);	

	DesenhaRobo();

	glutSwapBuffers();
}


// inicializa parâmetros de rendering
void Inicializa (void)
{ 

	// carrega uso de textura 
    LoadGLTextures();				
	// habilita uso de texture mapping
    glEnable(GL_TEXTURE_2D);			

	GLfloat luzAmbiente[4]={0.3,0.3,0.3,1.0}; 

	// cor
	GLfloat luzDifusa[4]={0.5,0.5,0.5,1.0};	   	

	// brilho
	GLfloat luzEspecular[4]={1.0, 1.0, 1.0, 1.0};	
	GLfloat posicaoLuz[4]={0.0, 300.0, 0.0, 1.0};

	// capacidade de brilho do material
	GLfloat especularidade[4]={1.0,1.0,1.0,1.0}; 
	GLint especMaterial = 60;

	// especifica que a cor de fundo da janela será preta
	glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
	
	// habilita o modelo de colorização de Gouraud
	glShadeModel(GL_SMOOTH);

	// define a refletância do material 
	glMaterialfv(GL_FRONT,GL_SPECULAR, especularidade);
	// define a concentração do brilho
	glMateriali(GL_FRONT,GL_SHININESS,especMaterial);

	// ativa o uso da luz ambiente 
	glLightModelfv(GL_LIGHT_MODEL_AMBIENT, luzAmbiente);

	// define os parâmetros da luz de número 0
	glLightfv(GL_LIGHT0, GL_AMBIENT, luzAmbiente); 
	glLightfv(GL_LIGHT0, GL_DIFFUSE, luzDifusa );
	glLightfv(GL_LIGHT0, GL_SPECULAR, luzEspecular );
	glLightfv(GL_LIGHT0, GL_POSITION, posicaoLuz );

	// habilita a definição da cor do material a partir da cor corrente
	glEnable(GL_COLOR_MATERIAL);

	// habilita o uso de iluminação
	glEnable(GL_LIGHTING);  

	// habilita a luz de número 0
	glEnable(GL_LIGHT0);

	// habilita o depth-buffering
	glEnable(GL_DEPTH_TEST);

	angle=50;

}


// funcao que calcula os capos equipotenciais
void campos(){

   	int v, i;   	float ti, td, fi, ri, Fx, Fz;

	if(sqrt ((rx1-xx1)*(rx1-xx1) + (rz1-zz1)*(rz1-zz1)) > 2 && temp == 0) {
	
		td = atan2 (zz1-rz1,xx1-rx1);
		Fx = 15 * cos (td);
		Fz = 15 * sin (td);
		for (i=0; i < OBS; i++) {
            
	    	ri = sqrt ( ((obs[i][1]-rz1)*(obs[i][1]-rz1)) + ((obs[i][0]-rx1)*(obs[i][0]-rx1))  )- 10 - 10;    /*Distancia entre o Robo e o Obstaculo.*/
	      	ti = atan2 (obs[i][1]-rz1, obs[i][0]-rx1); /*Teta (Td) REpulsivo*/
	      	v = (i%2==0)?1:-1;/*ramdonico de f entre 1 e -1*/
	      	Fx = Fx + (-fxy*(cos(ti)*ri) - v*sin(ti)) / (ri*ri) ;
	      	Fz = Fz + (-fxy*(sin(ti)*ri) + v*cos(ti)) / (ri*ri) ;
		}

		fi = atan2 (Fz,Fx);
		rx1 = rx1 + 1 * cos(fi);
		rz1 = rz1 + 1 * sin(fi);
		printf("%f  ,  %f\n",rx1,rz1);
   }
	else if(temp == 0){
		toca();
		temp++;
	}	
}

// função callback chamada pela GLUT a cada intervalo de tempo
void Timer(int value)
{

	campos();

    // redesenha o quadrado com as novas coordenadas 
    glutPostRedisplay();
    glutTimerFunc(50,Timer, 1);
}



// função usada para especificar o ponto de visualização
void EspecificaParametrosVisualizacao(void)
{
	// especifica sistema de coordenadas de projeção
	glMatrixMode(GL_PROJECTION);
	// inicializa sistema de coordenadas de projeção
	glLoadIdentity();

	// especifica a projeção perspectiva
	gluPerspective(angle,fAspect,0.1,1000);

	// especifica sistema de coordenadas do modelo
	glMatrixMode(GL_MODELVIEW);
	// inicializa sistema de coordenadas do modelo
	glLoadIdentity();

	// especifica posição do observador e do alvo
	gluLookAt(cx1, cy1, cz1, 100, 200, 100, 0,1,0);
}

// função callback chamada quando o tamanho da janela é alterado 
void AlteraTamanhoJanela(GLsizei w, GLsizei h)
{
	// Para previnir uma divisão por zero
	if ( h == 0 ) h = 1;

	// especifica o tamanho da viewport
	glViewport(0, 0, w, h);
 
	// calcula a correção de aspecto
	fAspect = (GLfloat)w/(GLfloat)h;

	EspecificaParametrosVisualizacao();
}


void GerenciaTeclado(unsigned char key, int x, int y) {

	int i;
	
    switch (key) {
            case 'w':	cz1 += 1;
						EspecificaParametrosVisualizacao();
						break;

            case 's':	cz1 -= 1;
						EspecificaParametrosVisualizacao();
                     	break;

            case 'a':	cx1 += 1;
						EspecificaParametrosVisualizacao();
                     	break;
            case 'd':
						cx1 -= 1; 
						EspecificaParametrosVisualizacao();
                     	break;
            case '-': {
						if(fxy >= 15)						
							fxy -= 2; 
                     	break;
			}
            case '=': {
						if(fxy <= 40)												
							fxy += 2; 
                     	break;
			}
    }

    glutPostRedisplay();
}



// programa principal
int main(int argc, char **argv) {

	int i;
	FILE *fp;
 	
	fp = fopen("entrada", "r");
    
    if (fp == NULL) {
        printf("ferrou");
        exit(1);       
    }
	else {
		for(i=0; i<OBS; i++)
			fscanf(fp, "%f %f\n", &obs[i][0], &obs[i][1]);
	}

	init();
	carregar();


	glutInit( &argc, argv );
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
  	
	//glutFullScreen();
	glutInitWindowSize(800,600);
	glutCreateWindow("Campos Potenciais");
	glutDisplayFunc(Desenha);  
	glutReshapeFunc(AlteraTamanhoJanela);
	glutTimerFunc(50, Timer, 1);
    glutKeyboardFunc(GerenciaTeclado);
	Inicializa();
	glutMainLoop();

	descarregar();
}
